/**
 * Implement the utilities for matrix computation. 
 * LU Decomposor is implemented. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file matrix_util.h
 */
#ifndef _XMESH_MATH_UTIL
#define _XMESH_MATH_UTIL


namespace math 
{
/**
 * LU Decomposor 
 */
template <typename value_, unsigned dim>
class LU_Decomposor
{
public:
	typedef value_          value_type ;   
	typedef matrix<value_type, dim, dim > matrix_type ;
	typedef vector<value_type, dim > vector_type ; 


	/**
	 * Decomposor constructor. 
	 */
	LU_Decomposor (const matrix_type _m) : m_matrix (_m)
	{
		this->decomposition();
	}

	/**
	 * Get LU decomposition result. 
	 * @return The constant reference of lu matrix. 
	 */
	const matrix_type & get_lu_matrix () const 
	{
		return this->m_lu_matrix; 
	}

	/**
	 * Calculate The inverse of matrix.
	 * @param The inverse of matrix. 
	 */
	void inverse(matrix_type & _inv)
	{
		unsigned n = _inv.get_row();
		matrix_type unit_matrix (n, n);
		for (unsigned i = 0; i < n; ++i ) 
		{
			for (unsigned j = 0; j < n; ++j ) 
			{
				unit_matrix [i][j] = value_type(0);
				unit_matrix [i][i] = value_type(1); 
			}
		}
		solve(unit_matrix, _inv); 
	}


	/**
	 * Calculate the determinant of matrix. 
	 * @return the det of matrix. 
	 */
	value_type det ()
	{
		value_type result = this->d ;
		for (unsigned i = 0; i < dim; ++i)
		{
			result *= m_lu_matrix(i,i);
		}
		return result; 
	}	

	/**
	 * Solve equation AX = B. 
	 * @param B Matrix on the right side. 
	 * @param X Linear equation solution. 
	 */
	template <unsigned dim2>
	void solve  (const matrix <value_type, dim, dim2> & b,  matrix <value_type, dim, dim2>& x)
	{
		unsigned n = dim ;
		unsigned m = dim2 ;

		vector_type col_vector ;
		for (unsigned j = 0; j < m; ++j )
		{
			for (unsigned i = 0; i < n; ++i ) col_vector[i] = b[i][j] ;
			solve (col_vector, col_vector);
			for (unsigned i = 0; i < n; ++i ) x[i][j] = col_vector[i];
		}
	}
	
	/**
	 * Solve equation Ax = b. Both x and b are vectors.
	 * @param b Vector on the right side. 
	 * @param x Linear equation solution. 
	 */
	void solve (vector_type & b, vector_type & x)
	{
		unsigned ii = 0; 
		value_type sum ; 
		unsigned n = dim;
		if (! (b.size() == n && x.size() == n))
		{
			throw ("The vector and the matrix's sizes do not match. ");
		}

		for (unsigned i = 0; i  < n; ++i) x[i] = b[i]; 
		for (unsigned i = 0; i < n ; ++i)
		{
		    unsigned ip = this->indx[i] ;
			value_type sum = x[ip] ;
			x[ip] = x[i] ;
			if (ii != 0)
				for (unsigned j = ii - 1; j < i; ++j ) sum -= m_lu_matrix[i][j] * x[j] ;
			else if (sum != value_type (0)) ii = i + 1;
			x[i] = sum; 
		}
		for (int i = n - 1 ; i >=0 ; --i)
		{
			sum = x[i];
			for (unsigned j = i + 1; j < n; ++j ) sum -= m_lu_matrix[i][j] * x[j]; 
			x[i] = sum / m_lu_matrix[i][i]; 
		}
	} 

private :
	/**
	 * Calculates LU Decomposition of the matrix. 
	 */
	matrix_type & decomposition ()
	{
		
		m_lu_matrix = m_matrix;
		unsigned n = dim; 
		d = value_type(1);
		std::vector<value_type> vv; 
		vv.resize(n); 
		indx.resize(n); 

		value_type big, temp; 
		for (unsigned i = 0; i < n; ++i )
		{
			big = value_type(0); 
			for (unsigned j = 0; j < n; ++j )
			{
				if ((temp = std::abs(m_lu_matrix(i,j)) ) > big ) big = temp; 
			}
			if (big == value_type(0)) throw ("Singular matrix in LUDecomposition");
			vv[i] = value_type(1) / big; 
		}

		for (unsigned k = 0; k < n; ++k)
		{
			big = value_type(0); 
			unsigned imax ;
			for (unsigned i = k ; i < n; ++i )
			{
				temp = vv[i] * std::abs (m_lu_matrix(i,k));
				if (temp > big)
				{
					big = temp; 
					imax = i; 
				}
			}
			if (!(k == imax))
			{
				for (unsigned j = 0; j < n ; ++j )
				{
					swap (m_lu_matrix (imax, j), m_lu_matrix(k,j));
				}
				d = -d ;
				vv[imax] = vv[k]; // Maybe it should be swap (vv[imax], vv[k]);
			}
			indx[k] = imax; 
//			if (m_lu_matrix[k][k] == ValueT(0)) m_lu_matrix[k][k] = TINY ; 

			for (unsigned i = k + 1; i < n; ++i)
			{
				temp = m_lu_matrix(i,k) /= m_lu_matrix(k,k);
				for (unsigned j = k+1; j < n; ++j)
				{
					m_lu_matrix(i,j) -= temp * m_lu_matrix(k,j);
				}
			}
		}
		return m_lu_matrix ;
	}


	/**
	 * Calculates LU Decomposition of the matrix. 
	 * @param _m The matrix to be decomposed.
	 * @param _l Low triangle matrix.
	 * @param _u Up triangle matrix. 
	 * @return None. 
	 */
	/*
	void LU_decomposition (matrix_type & _l, matrix_type & _u )
	{
		matrix_type & _m = this->m_matrix;
		if (! _m.is_square())
		{
			throw ("Not a square matrix. Cannot be decomposed. "); 
		}
		if ( ! (is_dim_match (_m, _l) && is_dim_match (_m, _u)))
		{
			throw ("Dimension is not matched.");
		}
		unsigned idx = _m.get_row();

		for (int i = 0; i < idx; ++i) _l(i,i) = ValueT(1);

		for (int j = 0; j < idx; ++j )
		{
			for (int i = 0 ; i <=j; ++i)
			{
				ValueT sum = 0; 
				for (int k = 0; k < i; ++k)
				{
					sum += _l (i,k) * _u (k,j) ;
				}
				_u(i,j) = _m(i,j) - sum; 
			}

			for (int i = j + 1; i < idx; ++i)
			{
				ValueT sum = ValueT(0);
				for (int k = 0; k < j; ++k)
				{
					sum += _l (i, k) * _u (k,j); 
				}
				_l ( i,j ) = ValueT(1) / (_u (j,j) ) * (_m (i,j) - sum) ;
			}
		}
	}
	*/

private:
	matrix_type m_matrix; 
	matrix_type m_lu_matrix; 
	std::vector<value_type> indx; 
	value_type d ;

};


/**
 * Rotation tools for matrix and vector. 
 * Rotate the matrix counterclockwise for angle rot. 
 * The matrix M is 2 x n . 
 * Rotation matrix is R = [cos t, - sin t; sin t, cos t]
 * The matrix rotated is R' = RM.
 * @param M Matrix to be rotated. 
 * @param rot Rotation angle. 
 */
template <typename value_type, unsigned col>
inline void MatrixRotate (matrix<value_type, 2, col> & M, double rot)
{
	matrix<value_type, 2, 2> R; 
	R = cos (rot) , - sin(rot), sin (rot), cos (rot) ;
	M = R * M ; 
}

/**
 * Counterclockwise rotation col vector of matrix about X axis in right-hand coordinate along the positive direction.  
 * The matrix M is 3 x n . 
 * Rotation matrix is R = [1, 0, 0; 0, cos t, - sin t; 0, sin t, cos t]
 * The matrix rotated is R' = RM.
 * @param M Matrix to be rotated. 
 * @param rot Rotation angle. 
 */
template <typename value_type, unsigned col>
	inline void MatrixRotateX (matrix<value_type, 3, col> & M, double rot)
{
	matrix<value_type, 3, 3> R;
	R = 1, 0, 0, 0, cos (rot), - sin (rot), 0, sin (rot), cos (rot) ;
	M = R * M ; 
}

/**
 * Counterclockwise rotation col vector of matrix about X axis in right-hand coordinate along the positive direction.  
 * The matrix M is 3 x n . 
 * Rotation matrix is R = [1, 0, 0; 0, cos t, - sin t; 0, sin t, cos t]
 * The matrix rotated is R' = RM.
 * @param M Matrix to be rotated. 
 * @param rot Rotation angle. 
 */
template <typename value_type, unsigned dim>
	inline vector<value_type, dim> VectorRotateX(const vector<value_type, dim> & M, const value_type & rot)
{
	matrix<value_type, 3, 3> R;
	R = 1, 0, 0, 0, cos (rot), - sin (rot), 0, sin (rot), cos (rot) ;
	value_type r0 = R[0][0] * M[0] + R[0][1] * M[1] + R[0][2] * M[2] ;
	value_type r1 = R[1][0] * M[0] + R[1][1] * M[1] + R[1][2] * M[2] ;
	value_type r2 = R[2][0] * M[0] + R[2][1] * M[1] + R[2][2] * M[2] ;
	
	return vector<value_type, dim> (r0, r1, r2); 
}


/**
 * Counterclockwise rotation col vector of matrix about Y axis in right-hand coordinates along the positive direction. 
 * The matrix M is 3 x n . 
 * Rotation matrix is R = [cos t, 0, sin t; 0, 1, 0; - sin t, 0, cos t]
 * The matrix rotated is R' = RM.
 * @param M Matrix to be rotated. 
 * @param rot Rotation angle. 
 */
template <typename value_type, unsigned col>
	inline void MatrixRotateY (matrix<value_type, 3, col> & M, double rot)
{
	matrix<value_type, 3, 3> R; 
	R = cos (rot), 0, sin (rot), 0, 1, 0, - sin (rot), 0, cos( rot); 
	M = R * M ; 
}


template <typename value_type, unsigned dim>
	inline vector<value_type, dim> VectorRotateY(const vector<value_type, dim> & M, const value_type & rot)
{
	matrix<value_type, 3, 3> R;
	R = cos (rot), 0, sin (rot), 0, 1, 0, - sin (rot), 0, cos( rot); 
	value_type r0 = R[0][0] * M[0] + R[0][1] * M[1] + R[0][2] * M[2] ;
	value_type r1 = R[1][0] * M[0] + R[1][1] * M[1] + R[1][2] * M[2] ;
	value_type r2 = R[2][0] * M[0] + R[2][1] * M[1] + R[2][2] * M[2] ;
	
	return vector<value_type, dim> (r0, r1, r2); 
}


/**
 * Counterclockwise rotation col vector of matrix about Z axis in right-hand coordinates along the positive direction. 
 * The matrix M is 3 x n . 
 * Rotation matrix is R = [cos t, - sin t, 0; sin t, cos t, 0;  0, 0, 1]. 
 * The matrix rotated is R' = RM.
 * @param M Matrix to be rotated. 
 * @param rot Rotation angle. 
 */
template <typename value_type, unsigned col>
	inline void MatrixRotateZ (matrix<value_type, 3, col> & M, double rot)
{
	matrix<value_type, 3,3> R;
	R = cos (rot), - sin (rot), 0, sin (rot), cos (rot), 0,  0, 0, 1 ;
	M = R * M ; 
}

template <typename value_type, unsigned dim>
	inline vector<value_type, dim> VectorRotateZ(const vector<value_type, dim> & M, const value_type & rot)
{
	matrix<value_type, 3, 3> R;
	R = cos (rot), - sin (rot), 0, sin (rot), cos (rot), 0,  0, 0, 1 ;

	value_type r0 = R[0][0] * M[0] + R[0][1] * M[1] + R[0][2] * M[2] ;
	value_type r1 = R[1][0] * M[0] + R[1][1] * M[1] + R[1][2] * M[2] ;
	value_type r2 = R[2][0] * M[0] + R[2][1] * M[1] + R[2][2] * M[2] ;
	
	return vector<value_type, dim> (r0, r1, r2); 
}

/**
 * Counterclockwise rotation col vector of matrix about a vector axis in right-hand coordinates along the positive direction. 
 * The matrix M is 3 x n . 
 * Rotation matrix is R = [cos (rot) + v[0] * v[0] * (1 - cos (rot)) , v[0] * v[1] * (1 - cos (rot)) - v[2] * sin(rot) , v[0] * v[2] * (1 - cos(rot) ) + v[1] * sin(rot); 
		v[0] * v[1] * (1 - cos(rot)) + v[2] * sin(rot), cos (rot) + v[1] * v[1] * (1 - cos (rot)), v[1] * v[2] * (1 - cos (rot)) - v[0] * sin(rot)  ;
		v[0] * v[2] * (1 - cos (rot)) - v[1] * sin(rot) , v[1] * v[2] * (1 - cos (rot)) + v[0] * sin(rot) , cos (rot) + v[2] * v[2] * (1 - cos (rot))]. 
 * The matrix rotated is R' = RM.
 * @param M Matrix to be rotated. 
 * @param _v Vector respected. 
 * @param rot Rotation angle. 
 */
template <typename value_type, unsigned col>
	inline void MatrixRotate3D (matrix<value_type, 3, col> & M, const vector<value_type, 3> & _v,  double rot)
{
	matrix<value_type, 3, 3> R;
	vector<value_type, 3> v (_v);
	v.normalize();

	R = cos (rot) + v[0] * v[0] * (1 - cos (rot)) , v[0] * v[1] * (1 - cos (rot)) - v[2] * sin(rot) , v[0] * v[2] * (1 - cos(rot) ) + v[1] * sin(rot), 
		v[0] * v[1] * (1 - cos(rot)) + v[2] * sin(rot), cos (rot) + v[1] * v[1] * (1 - cos (rot)), v[1] * v[2] * (1 - cos (rot)) - v[0] * sin(rot) , 
		v[0] * v[2] * (1 - cos (rot)) - v[1] * sin(rot) , v[1] * v[2] * (1 - cos (rot)) + v[0] * sin(rot) , cos (rot) + v[2] * v[2] * (1 - cos (rot));

	M = R * M ; 
}

/**
 * Reflection 3D. 
 * @param in_dir, the direction of the source light. 
 * @param normal, the normal of the reflection plane. Point to the outside of the plane. 
 */
template <typename value_type>
vector<value_type, 3> reflection(const vector <value_type, 3> & in_dir, const vector<value_type, 3> & normal )
{
	vector<value_type, 3> n = normal; 
	n.normalize() ; 
	return in_dir - 2.0 * ( (n * in_dir) * n ) ;
}

/**
 * A function for creating a rotation matrix that rotates a vector called. "from" into another vector called "to".
 @param from[3], 
 @param to[3] which both must be *normalized* non-zero vectors
 @return mtx[3][3] -- a 3x3 matrix in colum-major form
 @link Refer to http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/fromtorot.txt. 
 */
template <typename value_type>
	inline void fromToRotation(const vector<value_type, 3> & from, const vector<value_type, 3> &  to, matrix<value_type, 3, 3> & mtx) 
{

	vector<value_type, 3> v;
	value_type e, h, f;

	v = from ^ to ; //CROSS(v, from, to);
	e = from * to ; //e = DOT(from, to);
	f = (e < 0)? -e:e;
	if (f > value_type(1.0 - 0.000001))     /* "from" and "to"-vector almost parallel */
	{
		vector<value_type, 3> u, v; /* temporary storage vectors */
	    vector<value_type, 3> x;       /* vector most nearly orthogonal to "from" */
		value_type c1, c2, c3; /* coefficients for later use */
		int i, j;

		x[0] = (from[0] > 0.0)? from[0] : -from[0];
		x[1] = (from[1] > 0.0)? from[1] : -from[1];
		x[2] = (from[2] > 0.0)? from[2] : -from[2];

		if (x[0] < x[1])
		{
			if (x[0] < x[2])
			{
				x[0] = 1.0; x[1] = x[2] = 0.0;
			}
			else
			{
				x[2] = 1.0; x[0] = x[1] = 0.0;
			}
		}
		else
		{
			if (x[1] < x[2])
			{
				x[1] = 1.0; x[0] = x[2] = 0.0;
			}
			else
			{
				x[2] = 1.0; x[0] = x[1] = 0.0;
			}
		}

		u[0] = x[0] - from[0]; u[1] = x[1] - from[1]; u[2] = x[2] - from[2];
		v[0] = x[0] - to[0];   v[1] = x[1] - to[1];   v[2] = x[2] - to[2];

		c1 = 2.0 / DOT(u, u);
		c2 = 2.0 / DOT(v, v);
		c3 = c1 * c2  * DOT(u, v);

		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++) {
				mtx[i][j] =  - c1 * u[i] * u[j]
					- c2 * v[i] * v[j]
					+ c3 * v[i] * u[j];
			}
			mtx[i][i] += 1.0;
		}
	}
	else  /* the most common case, unless "from"="to", or "from"=-"to" */
	{
#if 0
		/* unoptimized version - a good compiler will optimize this. */
		/* h = (1.0 - e)/DOT(v, v); old code */
		h = 1.0/(1.0 + e);      /* optimization by Gottfried Chen */
		mtx[0][0] = e + h * v[0] * v[0];
		mtx[0][1] = h * v[0] * v[1] - v[2];
		mtx[0][2] = h * v[0] * v[2] + v[1];

		mtx[1][0] = h * v[0] * v[1] + v[2];
		mtx[1][1] = e + h * v[1] * v[1];
		mtx[1][2] = h * v[1] * v[2] - v[0];

		mtx[2][0] = h * v[0] * v[2] - v[1];
		mtx[2][1] = h * v[1] * v[2] + v[0];
		mtx[2][2] = e + h * v[2] * v[2];
#else
		/* ...otherwise use this hand optimized version (9 mults less) */
		float hvx, hvz, hvxy, hvxz, hvyz;
		/* h = (1.0 - e)/DOT(v, v); old code */
		h = 1.0/(1.0 + e);      /* optimization by Gottfried Chen */
		hvx = h * v[0];
		hvz = h * v[2];
		hvxy = hvx * v[1];
		hvxz = hvx * v[2];
		hvyz = hvz * v[1];
		mtx[0][0] = e + hvx * v[0];
		mtx[0][1] = hvxy - v[2];
		mtx[0][2] = hvxz + v[1];

		mtx[1][0] = hvxy + v[2];
		mtx[1][1] = e + h * v[1] * v[1];
		mtx[1][2] = hvyz - v[0];

		mtx[2][0] = hvxz - v[1];
		mtx[2][1] = hvyz + v[0];
		mtx[2][2] = e + hvz * v[2];
#endif
	}
}

template <typename value_type>
	matrix<value_type, 3, 3> star (const vector <value_type ,3 > & v)
{
	matrix <value_type , 3, 3> m ; 
	m = 0, -v.z , v.y , 
		v.z , 0, -v.x , 
		-v.y, v.x , 0 ;
	return m ;
}
/**
 * convert  rotation matrix to quaterion;
 * quaterion is in format : x + i y + j z + k w; 
 */
template <typename value_type>
	vector <value_type, 4> matrix2quaterion (const matrix <value_type , 3, 3> & m ) 
{
	// quaterion is in format : x + i y + j z + k w; 

	vector <value_type, 4> q ;
	value_type tr, s ;
	tr = m[0][0] + m [1][1] + m[2][2] ;
	if (tr > 1e-7 )
	{
		s = sqrt ( tr + 1);
		q.x = value_type(0.5 ) * s ;
		s = 0.5 / s; 
		q.y = (m[2][1] - m[1][2]) * s ;
		q.z = (m[0][2] - m[2][0]) * s ;
		q.w = (m[1][0] - m[0][1]) * s ; 
	}
	else 
	{
		int i = 0; 
		if (m[1][1] > m[0][0]) i = 1 ; 
		if (m[2][2] > m[i][i]) i = 2 ;

		switch (i) 
		{
		case 0 : 
			s = sqrt ((m[0][0] - m[1][1] - m[2][2]) + 1) ;
			q.y = 0.5 * s ; s = 0.5 / s; 
			q.z = (m[0][1] + m[1][0] ) * s; q.w = (m[2][0] + m[0][2] ) * s ; q.x = (m[2][1] - m[1][2] ) * s ;		
			break; 
		case 1 : 
			s = sqrt ((m[1][1] - m[2][2] - m[0][0]) + 1) ;
			q.z = 0.5 * s ; s = 0.5 / s; 
			q.w = (m[1][2] + m[2][1] ) * s; q.y = (m[0][1] + m[1][0] ) * s ; q.x = (m[0][2] - m[2][0] ) * s ;		
			break; 
		case 2 : 
			s = sqrt ((m[2][2] - m[0][0] - m[1][1]) + 1) ;
			q.w = 0.5 * s ; s = 0.5 / s; 
			q.y = (m[2][0] + m[0][2] ) * s; q.z = (m[1][2] + m[2][1] ) * s ; q.x = (m[1][0] - m[0][1] ) * s ;		
			break; 
		}
	}
	return q; 
}

/**
 * convert a quaterion to rotation matrix ;
 * quaterion is in format : x + i y + j z + k w; 
 */
template <typename value_type>
	matrix <value_type , 3, 3> quaterion2matrix (const vector <value_type , 4> & q) 
{
	matrix <value_type , 3, 3> mat ;
    value_type xx      = q.y * q.y;
    value_type xy      = q.y * q.z;
    value_type xz      = q.y * q.w;
    value_type xw      = q.y * q.x;
    value_type yy      = q.z * q.z;
    value_type yz      = q.z * q.w;
    value_type yw      = q.z * q.x;
    value_type zz      = q.w * q.w;
    value_type zw      = q.w * q.x;

    mat[0][0]  = 1 - 2.0 * ( yy + zz );
    mat[0][1]  =     2.0 * ( xy - zw );
    mat[0][2]  =     2.0 * ( xz + yw );
    mat[1][0]  =     2.0 * ( xy + zw );
    mat[1][1]  = 1 - 2.0 * ( xx + zz );
    mat[1][2]  =     2.0 * ( yz - xw );
    mat[2][0]  =     2.0 * ( xz - yw );
    mat[2][1]  =     2.0 * ( yz + xw );
    mat[2][2] = 1 - 2.0 * ( xx + yy );

	return mat ;
}


}
/*
template <typename value_type>
class MatrixRotator2D 
{
public:
	typedef value_type value_type ;
	typedef Vector <value_type> vector_type ; 
	typedef DenseMatrix <value_type> matrix_type ; 

	VectorRotator2D (const vector_type & _v) : m_vector ( _v)
	{
	}

	Rotate
private :
	vector_type m_vector; 
};
*/
#endif
